#include "mfcc.h"

#define PREEMPHASIS_ALPHA 0.97
#define M_PI		3.14159265358979323846
//窗口类型
#define winType 0.54
//采样率
#define SAMPLE_RATE 16000
//梅尔滤波器组个数
#define NUM_MEL_FILTERS 26
//最终有效mfcc个数
#define NUM_MFCC_COEFFS 13
//每一帧的大小
#define FRAME_SIZE 512


//static float mel_filters[NUM_MEL_FILTERS][FRAME_SIZE / 2 + 1];

//static float mel_filters[0][0 / 2 + 1];
static float ** mel_filters;
static void*scratch;
static kiss_fftr_cfg cfg;
//傅里叶变换初始化 
void fft_init(){
	size_t scratch_size = 0;
	cfg = kiss_fftr_alloc(FRAME_SIZE, 0, NULL, &scratch_size);
	if (cfg != NULL) {
	    printf( "Kiss memory sizing failed.\n");
	}
	scratch = malloc(scratch_size);
	cfg = kiss_fftr_alloc(FRAME_SIZE, 0,scratch, &scratch_size);

} 

// 预加重函数
void pre_emphasis(float *signal, int length, float alpha, float *output) {
    output[0] = signal[0];
    for (int i = 1; i < length; i++) {
        output[i] = signal[i] - alpha * signal[i - 1];
    }
}
//加窗函数 
static void hammingWindow(float*out,int window_size){
	const float arg = M_PI * 2.0;
	for(int i=0;i<window_size;i++){
		float float_value = winType - ((1-winType)* cos(arg*i/window_size ));
		out[i]=out[i]*float_value;
	}
}

// 初始化 Mel 滤波器组
void init_mel_filters() {
    float low_freq = 0;
    int num=2595;
    float high_freq = SAMPLE_RATE / 2;
    float low_mel = num * log10(1 + low_freq / 700);
    float high_mel = num * log10(1 + high_freq / 700);

    //static float mel_filters[NUM_MEL_FILTERS][FRAME_SIZE / 2 + 1];
    mel_filters=(float**)malloc(NUM_MEL_FILTERS*sizeof(float*));
    for(int i=0;i<NUM_MEL_FILTERS;i++){
        mel_filters[i]=(float*)malloc((FRAME_SIZE / 2 + 1)*sizeof(float));
    }

    //全部初始化为0
    for(int i=0;i<NUM_MEL_FILTERS;i++){
        for(int j=0;j<FRAME_SIZE / 2 + 1;j++){
            mel_filters[i][j]=0.0;
        }
    }


    int bins[NUM_MEL_FILTERS+2]={0};
    for (int i = 0; i < NUM_MEL_FILTERS+1; i++) {
        float mel = low_mel + (high_mel - low_mel) * (i+1) / (NUM_MEL_FILTERS + 1);
        float freq = 700 * (pow(10,(mel / num)) - 1);
        int bin = (int)((FRAME_SIZE + 1) * freq / SAMPLE_RATE);
        bins[i+1]=bin;
    }
    for(int j=0;j<NUM_MEL_FILTERS;j++){
    	for(int i=bins[j];i<bins[j+1];i++){
    		mel_filters[j][i]=(i - bins[j])*1.0 / (bins[j+1]-bins[j]);
		}
		for(int i=bins[j+1];i<bins[j+2];i++){
			mel_filters[j][i]=(bins[j+2]-i)*1.0 / (bins[j+2]-bins[j+1]);
		}
	}

}

//快速傅里叶变换 
static void fft(float *input,float*output){
    float fft_in[FRAME_SIZE];
    kiss_fft_cpx fft_out[FRAME_SIZE/2+1];
    kiss_fftr(cfg, input, fft_out);
    for(int i=0;i<FRAME_SIZE/2+1;i++){
    	output[i]=(fft_out[i].r*fft_out[i].r+fft_out[i].i*fft_out[i].i)/FRAME_SIZE;
	}

}

// 计算一维 DCT 变化 
static void dct_old(float *input, float *output, int N) {
    for (int u = 0; u < N; u++) {
        float alpha = (u == 0) ? sqrt(1.0 / N) : sqrt(2.0 / N);
        float sum = 0.0;
        for (int x = 0; x < N; x++) {
            sum += input[x] * cos((M_PI * u * (2 * x + 1)) / (2 * N));
        }
        output[u] = alpha * sum;
    }
}

// 计算一维 DCT 变化 
static void dct_old2(float *input, float *output, int N) {
    float t1=sqrt(1.0 / N);
    float t2=sqrt(2.0 / N);
    int n2=2*N;
    for (int u = 0; u < N; u++) {
        //float alpha = (u == 0) ? sqrt(1.0 / N) : sqrt(2.0 / N); //9638
        float alpha = (u == 0) ? t1 : t2; //dct:9633
        float sum = 0.0;
        float t=M_PI * u;
        for (int x = 0; x < N; x++) {
            sum += input[x] * cos((t * (2 * x + 1)) / n2);
        }
        output[u] = alpha * sum;
    }
}


static void dct(float *input, float *output, int N) {
    const float t1 = sqrtf(1.0f / N); // 预计算常数
    const float t2 = sqrtf(2.0f / N); // 预计算常数
    const float pi_over_n2 = M_PI / (2 * N); // 预计算常数
    const int n2 = 2 * N;

    for (int u = 0; u < N; u++) {
        float alpha = (u == 0) ? t1 : t2; // 选择系数
        float sum = 0.0f;
        float t = pi_over_n2 * u; // 预计算 u 相关的常数

        for (int x = 0; x < N; x++) {
            float angle = t * (2 * x + 1); // 计算角度
            sum += input[x] * cosf(angle); // 累加结果
        }
        output[u] = alpha * sum; // 存储结果
    }
}


static void lifter(float*data){
	int L=22;
	for(int i=0;i<NUM_MFCC_COEFFS;i++){
		float lift = 1 + (L/2.0)*sin(M_PI*i/L);
		data[i]=data[i]*lift;
	}
	
} 


//单帧数据进行MFCC变换 
void mfcc(float*dataProcess,float*outputData){
	//加窗 
    hammingWindow(dataProcess,FRAME_SIZE);
    //fft
	float powspec[FRAME_SIZE/2+1];
	fft(dataProcess,powspec);
	//计算总功率 
	double sum=0;
	for(int i=0;i<FRAME_SIZE/2+1;i++){
		sum+=powspec[i];
	}
	float mel_spectrum[NUM_MEL_FILTERS] = {0};
    for (int i = 0; i < NUM_MEL_FILTERS; i++) {
        for (int j = 0; j <= FRAME_SIZE / 2; j++) {
            mel_spectrum[i] += powspec[j] * mel_filters[i][j];
        }
        mel_spectrum[i] = log(mel_spectrum[i] + 1e-6); // 对数运算
    }
    
    float output[NUM_MEL_FILTERS];
    //离散DCT变换 
    dct(mel_spectrum,output,NUM_MEL_FILTERS);
    lifter(output); 
    output[0]=log(sum);
    //最后去前13位有效数据 
	for(int i=0;i<NUM_MFCC_COEFFS;i++){
        outputData[i]=output[i];
	}
} 
